function [w_next, theta_next, Err_w_sum] = PID_Pos_Loop(w_now, theta_now, Err_w_sum, motor, n, Kp, Kv, Tv, w_expect, p_expect, step_time)
%实现PID位置环 速度模式
%w_now为当前角速度，theta_now为当前角度,Err_w_sum为角速度误差累计，m为连杆质量，L为连杆质心到转子距离, g为重力加速度,,n为传动比,Kv,Tv为PI参数, Kmv, Kdv, Tmv为系统增益, w_expect为期望角速度, step_time为仿真步长
Err_p = n*p_expect - n*theta_now;           %计算位置误差
Err_w = Err_p*Kp + n*w_expect - n*w_now;           %计算速度误差值
Err_w_sum = Err_w_sum + Kv*Err_w;       %累计积分误差
Uc = Kv*Tv*Err_w + Err_w_sum*step_time;    %PI环节输出结果
td = motor.m*motor.g*motor.L*cos(theta_now)/n;              %干扰力矩
beta = (motor.Kmv*Uc - motor.Kdv*td - n*w_now)/motor.Tmv; %得到传动比前的角加速度
w_next = w_now + beta/n * step_time;       %得到下一时刻角速度
theta_next = w_now*step_time + theta_now;       %得到下一时刻角度

end
